#include "head.hpp"

void norm_R()
{
    vector<double> x = {0, 0.5, 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, 5, 5.5, 6, 6.5, 7, 7.5, 8, 8.5, 9, 9.5, 10};
    vector<double> y = {2.9, 2.7, 4.8, 5.3, 7.1, 7.6, 7.7, 7.6, 9.4, 9.0, 9.6, 10, 10.2, 9.7, 8.3, 8.4, 9.0, 8.3, 6.6, 6.7, 4.1};

    // 获得 A
    int N = x.size();
    int p = 2;
    vector<vector<double>> A(N, vector<double>(p + 1));
    for (int i=0;i<N;i++)
    {
        for (int j=0;j<p+1;j++)
        {
            A[i][j] = pow(x[i], j);
        }
    }

    QR(A);
    // 获取上三角部分
    vector<vector<double>> Lt(p+1, vector<double>(p+1));
    for (int i=0;i<p+1;i++)
    {
        for (int j=i;j<p+1;j++)
        {
            Lt[i][j] = A[i][j];
        }
    }
    cout << "R:" << endl;
    mat_out(Lt);
    cout << endl;

    vector<vector<double>> L = trans_mat(Lt);
    vector<vector<double>> LtL = mat_mult(Lt, L);
    double two_norm1 = sqrt(power(LtL));
    cout << "norm 1:" << two_norm1 << endl;

    vector<vector<double>> _L = inv_lower(L);
    vector<vector<double>> _Lt = trans_mat(_L);
    vector<vector<double>> _Lt_L = mat_mult(_L, _Lt);

    double two_norm2 = sqrt(power(_Lt_L));
    cout << "norm 2:" << two_norm2 << endl;

    cout << "condition R:" << two_norm1 * two_norm2 << endl;
}

void norm_G()
{
    vector<double> x = {0, 0.5, 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, 5, 5.5, 6, 6.5, 7, 7.5, 8, 8.5, 9, 9.5, 10};
    vector<double> y = {2.9, 2.7, 4.8, 5.3, 7.1, 7.6, 7.7, 7.6, 9.4, 9.0, 9.6, 10, 10.2, 9.7, 8.3, 8.4, 9.0, 8.3, 6.6, 6.7, 4.1};
    
    int N = x.size();
    int p = 2;
    vector<vector<double>> G(p + 1, vector<double>(p + 1));
    for (int i = 0; i < p + 1; i++)
    {
        for (int j = 0; j < p + 1; j++)
        {
            G[i][j] = inner(pow(x, i), pow(x, j));
        }
    }

    cout << "G:" << endl;
    mat_out(G);
    cout << endl;

    // ||G||_2
    vector<vector<double>> Gt = trans_mat(G);
    vector<vector<double>> GtG = mat_mult(Gt, G);
    double two_norm1 = sqrt(power(GtG));
    cout << "norm 1:" << two_norm1 << endl;

    // ||G^-1||_2
    cholesky(G);
    // 获取下三角部分
    vector<vector<double>> L = lower_mat(G);
    vector<vector<double>> _L = inv_lower(L);
    vector<vector<double>> _Lt = trans_mat(_L);
    vector<vector<double>> _G = mat_mult(_Lt, _L);

    vector<vector<double>> _Gt = trans_mat(_G);
    vector<vector<double>> _Gt_G = mat_mult(_Gt, _G);
    double two_norm2 = sqrt(power(_Gt_G));
    cout << "norm 2:" << two_norm2 << endl;

    cout << "condition G:" << two_norm1 * two_norm2 << endl;
}

void test_power()
{
    // 测试幂法
    vector<vector<double>> A = {{0, -3, -1}, {-1, 0, 0}, {0, -1, 0}};
    cout << power(A) << endl;
}

int main()
{
    // 最小二乘法求解
    vector<double> x = {0, 0.5, 1, 1.5, 2, 2.5, 3, 3.5, 4, 4.5, 5, 5.5, 6, 6.5, 7, 7.5, 8, 8.5, 9, 9.5, 10};
    vector<double> y = {2.9, 2.7, 4.8, 5.3, 7.1, 7.6, 7.7, 7.6, 9.4, 9.0, 9.6, 10, 10.2, 9.7, 8.3, 8.4, 9.0, 8.3, 6.6, 6.7, 4.1};

    // vector<vector<double>> A = {{4, -2, 4, 2}, {-2, 10, -2, -7}, {4, -2, 8, 4}, {2, -7, 4, 7}};
    // vector<double> b = {8, 2, 16, 6};
    // vector<double> x = least_square(A, b);
    // vec_out(x);

    // 获得 A
    int N = x.size();
    int p = 2;
    vector<vector<double>> A(N, vector<double>(p + 1));
    for (int i=0;i<N;i++)
    {
        for (int j=0;j<p+1;j++)
        {
            A[i][j] = pow(x[i], j);
        }
    }

    vector<double> c = least_square(A, y);
    vec_out(c);

    cout << endl;
    norm_G();
    cout << endl;
    norm_R();

    return 0;
}